using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using BepuUtilities;
using System.Runtime.CompilerServices;
using BepuUtilities.Collections;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 约束类型批处理处理器的超类。负责解释生料批次,以便记账和解决。
    /// </summary>
    /// <remarks>
    /// <para>此类没有自己的实际状态。求解器为每个注册的约束类型创建唯一的类型处理器,并且所有实例都保存在非类型化内存中。
    /// 将功能与数据分开允许使用更少的GC跟踪实例,并允许更轻松地共享原始数据布局。</para>
    /// 例如,休眠模拟岛存储类型的批次,但它们的创建和使用方式不同-为方便起见,它们以每个岛为基础进行存储。
    /// 使用相同的系统,但引用类型为TypeBatches,数万个非活动孤岛将隐含数万个GC跟踪的对象。</para>
    /// 这是不能接受的,所以我们来了。
    /// <para>从概念上讲,您可以认为求解器的TypeProcessors数组类似于C函数指针。</para>
    /// </remarks>
    public abstract class TypeProcessor
    {
        // TODO：将其放在基类中实际上会使某些特殊约束类型的实现复杂化。考虑一个包含以下内容的"衔接"子求解器
        // N Body,对于N>Vector<Float>.Count*2。在这种情况下,您可能希望在内部执行SIMD,因此在此级别上不会有"包"。以后再担心这个吧。

        // 我们根据约束缓存类型ID和正文,以避免虚拟调用。
        protected int typeId;
        protected int bodiesPerConstraint;
        public int TypeId { get { return typeId; } }
        /// <summary>
        /// 获取与此类型处理器中的每个约束关联的体数。
        /// </summary>
        public int BodiesPerConstraint { get { return bodiesPerConstraint; } }
        /// <summary>
        /// 获取此类型处理器中的每个约束约束的自由度数。等于累积的脉冲中的条目数量。
        /// </summary>
        public int ConstrainedDegreesOfFreedom { get; private set; }
        protected abstract int InternalBodiesPerConstraint { get; }
        protected abstract int InternalConstrainedDegreesOfFreedom { get; }

        public void Initialize(int typeId)
        {
            this.typeId = typeId;
            this.bodiesPerConstraint = InternalBodiesPerConstraint;
            this.ConstrainedDegreesOfFreedom = InternalConstrainedDegreesOfFreedom;
        }

        /// <summary>
        /// 在批处理中分配槽。
        /// </summary>
        /// <param name="typeBatch">键入要分配的批次。</param>
        /// 要分配的约束的<param name="handle">句柄。建立从分配的约束到其句柄的链接。</param>
        /// 指向正文索引列表的<param name="bodyIndices">指针(不是句柄！)Count等于类型Batch的预期涉及正文数。</param>
        /// <param name="pool">必须调整批处理类型大小时使用的分配提供程序。</param>
        /// <return>批处理中槽的索引。</return>
        public unsafe abstract int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool);
        public abstract void Remove(ref TypeBatch typeBatch, int index, ref Buffer<ConstraintLocation> handlesToConstraints);

        /// <summary>
        /// 将约束从一个ConstraintBatch的TypeBatch移动到另一个相同类型的ConstraintBatch的TypeBatch。
        /// </summary>
        /// <param name="sourceBatchIndex">拥有作为约束传输源的批处理类型的批处理的索引。</param>
        /// <param name="indexInTypeBatch">要在当前类型批处理中移动的约束的索引。</param>
        /// <param name="solver">拥有批的规划求解程序。</param>
        /// <param name="Body">拥有所有约束正文的Body集。</param>
        /// <param name="targetBatchIndex">要将约束复制到的求解器中ConstraintBatch的索引。</param>
        public unsafe abstract void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex);

        public abstract void EnumerateConnectedBodyIndices<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<int>;
        [Conditional("DEBUG")]
        protected abstract void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes);
        public unsafe void EnumerateAccumulatedImpulses<TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach<float>
        {
            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var bundleIndex, out var innerIndex);
            var bundleSizeInFloats = ConstrainedDegreesOfFreedom * Vector<float>.Count;
            ValidateAccumulatedImpulsesSizeInBytes(bundleSizeInFloats * 4);
            var impulseAddress = (float*)typeBatch.AccumulatedImpulses.Memory + (bundleIndex * bundleSizeInFloats + innerIndex);
            enumerator.LoopBody(*impulseAddress);
            for (int i = 1; i < ConstrainedDegreesOfFreedom; ++i)
            {
                impulseAddress += Vector<float>.Count;
                enumerator.LoopBody(*impulseAddress);
            }
        }
        public abstract void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, float scale);
        public abstract void UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation);

        public abstract void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer<ConstraintLocation> handlesToConstraints);

        internal abstract void GetBundleTypeSizes(out int bodyReferencesBundleSize, out int prestepBundleSize, out int accumulatedImpulseBundleSize);

        internal abstract void GenerateSortKeysAndCopyReferences(
            ref TypeBatch typeBatch,
            int bundleStart, int localBundleStart, int bundleCount,
            int constraintStart, int localConstraintStart, int constraintCount,
            ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache);

        internal abstract void CopyToCache(
            ref TypeBatch typeBatch,
            int bundleStart, int localBundleStart, int bundleCount,
            int constraintStart, int localConstraintStart, int constraintCount,
            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache);

        internal abstract void Regather(
            ref TypeBatch typeBatch,
            int constraintStart, int constraintCount, ref int firstSourceIndex,
            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer bodyReferencesCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache,
            ref Buffer<ConstraintLocation> handlesToConstraints);

        internal unsafe abstract void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList<ConstraintHandle> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch);

        internal unsafe abstract void CopySleepingToActive(
            int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetBatchIndex, int targetTypeBatchIndex,
            int sourceStart, int targetStart, int count, Bodies bodies, Solver solver);


        internal unsafe abstract void AddWakingBodyHandlesToBatchReferences(ref TypeBatch typeBatch, ref IndexSet targetBatchReferencedHandles);

        [Conditional("DEBUG")]
        internal abstract void VerifySortRegion(ref TypeBatch typeBatch, int bundleStartIndex, int constraintCount, ref Buffer<int> sortedKeys, ref Buffer<int> sortedSourceIndices);
        internal abstract int GetBodyReferenceCount(ref TypeBatch typeBatch, int body);

        public abstract void Initialize(ref TypeBatch typeBatch, int initialCapacity, BufferPool pool);
        public abstract void Resize(ref TypeBatch typeBatch, int newCapacity, BufferPool pool);

        public abstract void Prestep(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int exclusiveEndBundle);
        public abstract void WarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle);
        public abstract void SolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, int startBundle, int exclusiveEndBundle);

        public abstract void JacobiPrestep(ref TypeBatch typeBatch, Bodies bodies, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle);
        public abstract void JacobiWarmStart(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle);
        public abstract void JacobiSolveIteration(ref TypeBatch typeBatch, ref Buffer<BodyVelocity> bodyVelocities, ref FallbackTypeBatchResults jacobiResults, int startBundle, int exclusiveEndBundle);

        public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies bodies, float dt, float inverseDt, int startBundle, int end)
        {
            Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation.");
        }
    }

    /// <summary>
    /// 定义从类型批处理中的正文引用创建排序关键字的函数。由约束布局优化使用。
    /// </summary>
    public interface ISortKeyGenerator<TBodyReferences> where TBodyReferences : unmanaged
    {
        int GetSortKey(int constraintIndex, ref Buffer<TBodyReferences> bodyReferences);
    }

    // 请注意,这里在类型级别使用泛型的唯一原因是避免为每个单独的函数指定泛型。它在功能上是等效的,但这只是
    // 稍微降低了语法噪音。
    // 真的,您可以使用一组合成的静电泛型帮助器。
    public abstract class TypeProcessor<TBodyReferences, TPrestepData, TProjection, TAccumulatedImpulse> : TypeProcessor where TBodyReferences : unmanaged where TPrestepData : unmanaged where TProjection : unmanaged where TAccumulatedImpulse : unmanaged
    {
        protected override int InternalConstrainedDegreesOfFreedom
        {
            get
            {
                // 我们对这里的内存布局做了一个假设。它不能保证是有效的,但是它碰巧适用于所有现有的和计划的约束。
                var dofCount = Unsafe.SizeOf<TAccumulatedImpulse>() / (4 * Vector<float>.Count);
                Debug.Assert(dofCount * 4 * Vector<float>.Count == Unsafe.SizeOf<TAccumulatedImpulse>(), "One of the assumptions of this DOF calculator is broken. Fix this!");
                return dofCount;
            }
        }
        protected override void ValidateAccumulatedImpulsesSizeInBytes(int sizeInBytes)
        {
            Debug.Assert(sizeInBytes == Unsafe.SizeOf<TAccumulatedImpulse>(), "Your assumptions about memory layout and size are wrong for this type! Fix it!");
        }

        public override unsafe void ScaleAccumulatedImpulses(ref TypeBatch typeBatch, float scale)
        {
            var dofCount = Unsafe.SizeOf<TAccumulatedImpulse>() / Unsafe.SizeOf<Vector<float>>();
            var broadcastedScale = new Vector<float>(scale);
            ref var impulsesBase = ref Unsafe.AsRef<Vector<float>>(typeBatch.AccumulatedImpulses.Memory);
            for (int i = 0; i < dofCount; ++i)
            {
                Unsafe.Add(ref impulsesBase, i) *= broadcastedScale;
            }
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void AddBodyReferencesLane(ref TBodyReferences bundle, int innerIndex, int* bodyIndices)
        {
            // jit应该能够合并几乎所有与大小相关的计算和处理小提琴。
            ref var start = ref Unsafe.As<TBodyReferences, int>(ref bundle);
            ref var targetLane = ref Unsafe.Add(ref start, innerIndex);
            var stride = Vector<int>.Count;
            // 我们假设Body References结构是在内存中组织的,就像Bundle0、Inner0、.捆绑包N,内部,计数
            // 假设连续存储,则COUNT位于START+STRIDE*Bodycount。
            var bodyCount = Unsafe.SizeOf<TBodyReferences>() / (stride * sizeof(int));
            targetLane = *bodyIndices;
            for (int i = 1; i < bodyCount; ++i)
            {
                Unsafe.Add(ref targetLane, i * stride) = bodyIndices[i];
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        protected static int GetCountInBundle(ref TypeBatch typeBatch, int bundleStartIndex)
        {
            // TODO：可能需要在此检查codegen。MIN与EXPLICIT BRANCH。从理论上讲,它可以无分支地做到这一点。
            return Math.Min(Vector<float>.Count, typeBatch.ConstraintCount - (bundleStartIndex << BundleIndexing.VectorShift));
        }


        public unsafe sealed override int Allocate(ref TypeBatch typeBatch, ConstraintHandle handle, int* bodyIndices, BufferPool pool)
        {
            Debug.Assert(typeBatch.BodyReferences.Allocated, "Should initialize the batch before allocating anything from it.");
            if (typeBatch.ConstraintCount == typeBatch.IndexToHandle.Length)
            {
                InternalResize(ref typeBatch, pool, typeBatch.ConstraintCount * 2);
            }
            var index = typeBatch.ConstraintCount++;
            typeBatch.IndexToHandle[index] = handle;
            BundleIndexing.GetBundleIndices(index, out var bundleIndex, out var innerIndex);
            ref var bundle = ref Buffer<TBodyReferences>.Get(ref typeBatch.BodyReferences, bundleIndex);
            AddBodyReferencesLane(ref bundle, innerIndex, bodyIndices);
            // 清除插槽的累积脉冲。后备存储器可以被初始化为任何值。
            GatherScatter.ClearLane<TAccumulatedImpulse, float>(ref Buffer<TAccumulatedImpulse>.Get(ref typeBatch.AccumulatedImpulses, bundleIndex), innerIndex);
            var bundleCount = typeBatch.BundleCount;
            Debug.Assert(typeBatch.PrestepData.Length >= bundleCount * Unsafe.SizeOf<TPrestepData>());
            Debug.Assert(typeBatch.Projection.Length >= bundleCount * Unsafe.SizeOf<TProjection>());
            Debug.Assert(typeBatch.BodyReferences.Length >= bundleCount * Unsafe.SizeOf<TBodyReferences>());
            Debug.Assert(typeBatch.AccumulatedImpulses.Length >= bundleCount * Unsafe.SizeOf<TAccumulatedImpulse>());
            Debug.Assert(typeBatch.IndexToHandle.Length >= typeBatch.ConstraintCount);
            return index;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        protected static void CopyConstraintData(
             ref TBodyReferences sourceReferencesBundle, ref TPrestepData sourcePrestepBundle, ref TAccumulatedImpulse sourceAccumulatedBundle, int sourceInner,
             ref TBodyReferences targetReferencesBundle, ref TPrestepData targetPrestepBundle, ref TAccumulatedImpulse targetAccumulatedBundle, int targetInner)
        {
            // 请注意,我们不复制小版本数据。它从头开始重新生成每一帧。
            GatherScatter.CopyLane(ref sourceReferencesBundle, sourceInner, ref targetReferencesBundle, targetInner);
            GatherScatter.CopyLane(ref sourcePrestepBundle, sourceInner, ref targetPrestepBundle, targetInner);
            GatherScatter.CopyLane(ref sourceAccumulatedBundle, sourceInner, ref targetAccumulatedBundle, targetInner);
        }
        /// <summary>
        /// 用源数据覆盖目标约束槽中的所有数据。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        protected static void Move(
            ref TBodyReferences sourceReferencesBundle, ref TPrestepData sourcePrestepBundle, ref TAccumulatedImpulse sourceAccumulatedBundle, ConstraintHandle sourceHandle,
            int sourceInner,
            ref TBodyReferences targetReferencesBundle, ref TPrestepData targetPrestepBundle, ref TAccumulatedImpulse targetAccumulatedBundle, ref ConstraintHandle targetIndexToHandle,
            int targetInner, int targetIndex, ref Buffer<ConstraintLocation> handlesToConstraints)
        {
            CopyConstraintData(
                ref sourceReferencesBundle, ref sourcePrestepBundle, ref sourceAccumulatedBundle, sourceInner,
                ref targetReferencesBundle, ref targetPrestepBundle, ref targetAccumulatedBundle, targetInner);
            targetIndexToHandle = sourceHandle;
            handlesToConstraints[sourceHandle.Value].IndexInTypeBatch = targetIndex;
        }



        public sealed override unsafe void Scramble(ref TypeBatch typeBatch, Random random, ref Buffer<ConstraintLocation> handlesToConstraints)
        {
            // 这是一个纯粹的调试函数,用于比较缓存优化策略。表现并不重要。
            TPrestepData aPrestep = default;
            TAccumulatedImpulse aAccumulated = default;
            TBodyReferences aBodyReferences = default;
            ConstraintHandle aHandle;

            var prestepData = typeBatch.PrestepData.As<TPrestepData>();
            var accumulatedImpulses = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
            var bodyReferences = typeBatch.BodyReferences.As<TBodyReferences>();

            for (int a = typeBatch.ConstraintCount - 1; a >= 1; --a)
            {
                BundleIndexing.GetBundleIndices(a, out var aBundle, out var aInner);
                GatherScatter.CopyLane(ref bodyReferences[aBundle], aInner, ref aBodyReferences, 0);
                GatherScatter.CopyLane(ref prestepData[aBundle], aInner, ref aPrestep, 0);
                GatherScatter.CopyLane(ref accumulatedImpulses[aBundle], aInner, ref aAccumulated, 0);
                aHandle = typeBatch.IndexToHandle[a];

                var b = random.Next(a);
                BundleIndexing.GetBundleIndices(b, out var bBundle, out var bInner);
                // 把B移到A的槽里。
                Move(
                    ref bodyReferences[bBundle], ref prestepData[bBundle], ref accumulatedImpulses[bBundle], typeBatch.IndexToHandle[b], bInner,
                    ref bodyReferences[aBundle], ref prestepData[aBundle], ref accumulatedImpulses[aBundle], ref typeBatch.IndexToHandle[a], aInner, a, ref handlesToConstraints);
                // 将缓存的a移动到b的槽中。
                Move(
                    ref aBodyReferences, ref aPrestep, ref aAccumulated, aHandle, 0,
                    ref bodyReferences[bBundle], ref prestepData[bBundle], ref accumulatedImpulses[bBundle], ref typeBatch.IndexToHandle[b], bInner, b, ref handlesToConstraints);
            }
        }

        /// <summary>
        /// 从批处理中移除约束。
        /// </summary>
        /// <param name="index">要删除的约束的索引。</param>
        /// <param name="handlesToConstraints">求解器使用的约束映射句柄,删除时可通过交换进行修改。</param>
        public override unsafe void Remove(ref TypeBatch typeBatch, int index, ref Buffer<ConstraintLocation> handlesToConstraints)
        {
            Debug.Assert(index >= 0 && index < typeBatch.ConstraintCount, "Can only remove elements that are actually in the batch!");
            var lastIndex = typeBatch.ConstraintCount - 1;
            typeBatch.ConstraintCount = lastIndex;
            BundleIndexing.GetBundleIndices(lastIndex, out var sourceBundleIndex, out var sourceInnerIndex);

            ref var bodyReferences = ref Unsafe.As<byte, TBodyReferences>(ref *typeBatch.BodyReferences.Memory);
            if (index < lastIndex)
            {
                // 需要调换一下。
                ref var prestepData = ref Unsafe.As<byte, TPrestepData>(ref *typeBatch.PrestepData.Memory);
                ref var accumulatedImpulses = ref Unsafe.As<byte, TAccumulatedImpulse>(ref *typeBatch.AccumulatedImpulses.Memory);
                BundleIndexing.GetBundleIndices(index, out var targetBundleIndex, out var targetInnerIndex);
                Move(
                    ref Unsafe.Add(ref bodyReferences, sourceBundleIndex), ref Unsafe.Add(ref prestepData, sourceBundleIndex), ref Unsafe.Add(ref accumulatedImpulses, sourceBundleIndex),
                    typeBatch.IndexToHandle[lastIndex], sourceInnerIndex,
                    ref Unsafe.Add(ref bodyReferences, targetBundleIndex), ref Unsafe.Add(ref prestepData, targetBundleIndex), ref Unsafe.Add(ref accumulatedImpulses, targetBundleIndex),
                    ref typeBatch.IndexToHandle[index], targetInnerIndex, index,
                    ref handlesToConstraints);
            }
        }

        /// <summary>
        /// 将约束从一个ConstraintBatch的TypeBatch移动到另一个相同类型的ConstraintBatch的TypeBatch。
        /// </summary>
        /// <param name="sourceBatchIndex">拥有作为约束传输源的批处理类型的批处理的索引。</param>
        /// <param name="indexInTypeBatch">要在当前类型批处理中移动的约束的索引。</param>
        /// <param name="solver">拥有批的规划求解程序。</param>
        /// <param name="Body">拥有所有约束正文的Body集。</param>
        /// <param name="targetBatchIndex">要将约束复制到的求解器中ConstraintBatch的索引。</param>
        public unsafe override void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies bodies, int targetBatchIndex)
        {
            // 请注意,以下代码会做一些多余的工作。从技术上讲,做得比这更好是可能的,但它需要绕过大量的簿记。
            // 保持一切井然有序绝非易事,尤其是随着时间的推移,这会成为维护的噩梦。
            // 因此,考虑到压缩通常非常罕见(相对而言)并且高度延迟,我们将接受一些较小的开销。
            int bodiesPerConstraint = InternalBodiesPerConstraint;
            var bodyHandles = stackalloc int[bodiesPerConstraint];
            var bodyHandleCollector = new ActiveConstraintBodyHandleCollector(bodies, bodyHandles);
            EnumerateConnectedBodyIndices(ref typeBatch, indexInTypeBatch, ref bodyHandleCollector);
            Debug.Assert(targetBatchIndex <= solver.FallbackBatchThreshold,
                "Constraint transfers should never target the fallback batch. It doesn't have any body handles so attempting to allocate in the same way wouldn't turn out well.");
            // 在新批次中分配一个位置。请注意,它不会更改Solver中的Handle->约束映射;当我们调用下面的Solver.Remove时,这一点很重要。
            var constraintHandle = typeBatch.IndexToHandle[indexInTypeBatch];
            solver.AllocateInBatch(targetBatchIndex, constraintHandle, new Span<BodyHandle>(bodyHandles, bodiesPerConstraint), typeId, out var targetReference);

            BundleIndexing.GetBundleIndices(targetReference.IndexInTypeBatch, out var targetBundle, out var targetInner);
            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var sourceBundle, out var sourceInner);
            // 我们不会从旧的约束中提取描述或任何东西。这将需要从约束到"完整描述"的唯一映射。
            // 相反,我们只是直接从一个车道复制到另一个车道。
            // 请注意,我们省略了运行时生成的位-它们只会重新生成。
            CopyConstraintData(
                ref Buffer<TBodyReferences>.Get(ref typeBatch.BodyReferences, sourceBundle),
                ref Buffer<TPrestepData>.Get(ref typeBatch.PrestepData, sourceBundle),
                ref Buffer<TAccumulatedImpulse>.Get(ref typeBatch.AccumulatedImpulses, sourceBundle),
                sourceInner,
                ref Buffer<TBodyReferences>.Get(ref targetReference.TypeBatch.BodyReferences, targetBundle),
                ref Buffer<TPrestepData>.Get(ref targetReference.TypeBatch.PrestepData, targetBundle),
                ref Buffer<TAccumulatedImpulse>.Get(ref targetReference.TypeBatch.AccumulatedImpulses, targetBundle),
                targetInner);

            // 现在我们可以摆脱旧的分配方式了。
            // 请注意使用RemoveFromBatch而不是Remove。Solver.Remove将句柄返回给池,这是我们不需要的！
            // 在这里使用解算器级别的函数看起来可能有点奇怪,因为我们在很大程度上是在批处理上操作,直接处理解算器状态。
            // 但是,删除可能会导致需要回收资源的空批。
            // 而不是重新实现,我们只需重用求解器的版本。
            // 添加时不需要这种类型的资源清理-所需的一切都已经存在,并且没有任何东西会消失。
            solver.RemoveFromBatch(constraintHandle, sourceBatchIndex, typeId, indexInTypeBatch);

            // 别忘了保持求解器指针的一致性！我们绕过了通常的添加过程,所以还没有通知求解器。
            ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle.Value];
            constraintLocation.BatchIndex = targetBatchIndex;
            constraintLocation.IndexInTypeBatch = targetReference.IndexInTypeBatch;
            constraintLocation.TypeId = typeId;

        }

        void InternalResize(ref TypeBatch typeBatch, BufferPool pool, int constraintCapacity)
        {
            Debug.Assert(constraintCapacity >= 0, "The constraint capacity should have already been validated.");
            pool.ResizeToAtLeast(ref typeBatch.IndexToHandle, constraintCapacity, typeBatch.ConstraintCount);
            // 请注意,我们根据调整大小的约束容量构造捆绑容量。这意味着我们只需检查IndexToHandle容量
            // 在分配之前,这稍微简化了一些事情,但会占用一些内存。如果内存使用确实是一个问题,可以重新考虑这一点。
            var bundleCapacity = BundleIndexing.GetBundleCount(typeBatch.IndexToHandle.Length);
            // 请注意,不会复制投影。这是短暂的数据。(与上面的思路相同,如果内存有问题,我们可以只按需分配投影。)
            var bundleCount = typeBatch.BundleCount;
            pool.ResizeToAtLeast(ref typeBatch.Projection, bundleCapacity * Unsafe.SizeOf<TProjection>(), 0);
            pool.ResizeToAtLeast(ref typeBatch.BodyReferences, bundleCapacity * Unsafe.SizeOf<TBodyReferences>(), bundleCount * Unsafe.SizeOf<TBodyReferences>());
            pool.ResizeToAtLeast(ref typeBatch.PrestepData, bundleCapacity * Unsafe.SizeOf<TPrestepData>(), bundleCount * Unsafe.SizeOf<TPrestepData>());
            pool.ResizeToAtLeast(ref typeBatch.AccumulatedImpulses, bundleCapacity * Unsafe.SizeOf<TAccumulatedImpulse>(), bundleCount * Unsafe.SizeOf<TAccumulatedImpulse>());
        }

        public override void Initialize(ref TypeBatch typeBatch, int initialCapacity, BufferPool pool)
        {
            // 我们默认初始化类型Batch,因为调整大小会在决定复制多少时检查现有的分配状态。
            // 从技术上讲,我们可以使用特定于初始化的版本,但这并不重要。
            typeBatch = new TypeBatch();
            typeBatch.TypeId = TypeId;
            InternalResize(ref typeBatch, pool, initialCapacity);
        }

        public override void Resize(ref TypeBatch typeBatch, int desiredCapacity, BufferPool pool)
        {
            var desiredConstraintCapacity = BufferPool.GetCapacityForCount<int>(desiredCapacity);
            if (desiredConstraintCapacity != typeBatch.IndexToHandle.Length)
            {
                InternalResize(ref typeBatch, pool, desiredConstraintCapacity);
            }
        }


        internal sealed override void GetBundleTypeSizes(out int bodyReferencesBundleSize, out int prestepBundleSize, out int accumulatedImpulseBundleSize)
        {
            bodyReferencesBundleSize = Unsafe.SizeOf<TBodyReferences>();
            prestepBundleSize = Unsafe.SizeOf<TPrestepData>();
            accumulatedImpulseBundleSize = Unsafe.SizeOf<TAccumulatedImpulse>();
        }


        public sealed override void UpdateForBodyMemoryMove(ref TypeBatch typeBatch, int indexInTypeBatch, int bodyIndexInConstraint, int newBodyLocation)
        {
            BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
            // 请注意,这依赖于BodyReferences内存布局。它使用向量的步长基于bodyIndexInConstraint跳到下一个实体。
            ref var bundle = ref Unsafe.As<TBodyReferences, Vector<int>>(ref Buffer<TBodyReferences>.Get(ref typeBatch.BodyReferences, constraintBundleIndex));
            GatherScatter.Get(ref bundle, constraintInnerIndex + bodyIndexInConstraint * Vector<int>.Count) = newBodyLocation;
        }

        // 请注意,接下来的两个排序键用户需要通用的排序键实现;这避免了在每个对象级别上进行虚拟分派,同时仍然共享大部分逻辑。
        // 从技术上讲,我们可以强制在泛型类型级别定义TSortKeyGenerator,但这似乎有点.极端。
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        protected void GenerateSortKeysAndCopyReferences<TSortKeyGenerator>(
           ref TypeBatch typeBatch,
           int bundleStart, int localBundleStart, int bundleCount,
           int constraintStart, int localConstraintStart, int constraintCount,
           ref int firstSortKey, ref int firstSourceIndex, ref RawBuffer bodyReferencesCache)
            where TSortKeyGenerator : struct, ISortKeyGenerator<TBodyReferences>
        {
            var sortKeyGenerator = default(TSortKeyGenerator);
            var bodyReferences = typeBatch.BodyReferences.As<TBodyReferences>();
            for (int i = 0; i < constraintCount; ++i)
            {
                Unsafe.Add(ref firstSourceIndex, i) = localConstraintStart + i;
                Unsafe.Add(ref firstSortKey, i) = sortKeyGenerator.GetSortKey(constraintStart + i, ref bodyReferences);
            }
            var typedBodyReferencesCache = bodyReferencesCache.As<TBodyReferences>();
            bodyReferences.CopyTo(bundleStart, typedBodyReferencesCache, localBundleStart, bundleCount);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        protected void VerifySortRegion<TSortKeyGenerator>(ref TypeBatch typeBatch, int bundleStartIndex, int constraintCount, ref Buffer<int> sortedKeys, ref Buffer<int> sortedSourceIndices)
            where TSortKeyGenerator : struct, ISortKeyGenerator<TBodyReferences>
        {
            var sortKeyGenerator = default(TSortKeyGenerator);
            var previousKey = -1;
            var baseIndex = bundleStartIndex << BundleIndexing.VectorShift;
            var bodyReferences = typeBatch.BodyReferences.As<TBodyReferences>();
            for (int i = 0; i < constraintCount; ++i)
            {
                var sourceIndex = sortedSourceIndices[i];
                var targetIndex = baseIndex + i;
                var key = sortKeyGenerator.GetSortKey(baseIndex + i, ref bodyReferences);
                // 注意,该断言使用>=和NOT>;在同步的约束批处理中,主体引用不可能被复制,但是后备批处理可以有重复的。
                Debug.Assert(key >= previousKey, "After the sort and swap completes, all constraints should be in order.");
                Debug.Assert(key == sortedKeys[i], "After the swap goes through, the rederived sort keys should match the previously sorted ones.");
                previousKey = key;

            }
        }

        internal unsafe sealed override void CopyToCache(
            ref TypeBatch typeBatch,
            int bundleStart, int localBundleStart, int bundleCount,
            int constraintStart, int localConstraintStart, int constraintCount,
            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache)
        {
            typeBatch.IndexToHandle.CopyTo(constraintStart, indexToHandleCache, localConstraintStart, constraintCount);
            Unsafe.CopyBlockUnaligned(
                prestepCache.Memory + Unsafe.SizeOf<TPrestepData>() * localBundleStart,
                typeBatch.PrestepData.Memory + Unsafe.SizeOf<TPrestepData>() * bundleStart,
                (uint)(Unsafe.SizeOf<TPrestepData>() * bundleCount));
            Unsafe.CopyBlockUnaligned(
                accumulatedImpulsesCache.Memory + Unsafe.SizeOf<TAccumulatedImpulse>() * localBundleStart,
                typeBatch.AccumulatedImpulses.Memory + Unsafe.SizeOf<TAccumulatedImpulse>() * bundleStart,
                (uint)(Unsafe.SizeOf<TAccumulatedImpulse>() * bundleCount));
        }
        internal sealed override void Regather(
            ref TypeBatch typeBatch,
            int constraintStart, int constraintCount, ref int firstSourceIndex,
            ref Buffer<ConstraintHandle> indexToHandleCache, ref RawBuffer bodyReferencesCache, ref RawBuffer prestepCache, ref RawBuffer accumulatedImpulsesCache,
            ref Buffer<ConstraintLocation> handlesToConstraints)
        {
            var typedBodyReferencesCache = bodyReferencesCache.As<TBodyReferences>();
            var typedPrestepCache = prestepCache.As<TPrestepData>();
            var typedAccumulatedImpulsesCache = accumulatedImpulsesCache.As<TAccumulatedImpulse>();

            var typedBodyReferencesTarget = typeBatch.BodyReferences.As<TBodyReferences>();
            var typedPrestepTarget = typeBatch.PrestepData.As<TPrestepData>();
            var typedAccumulatedImpulsesTarget = typeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
            for (int i = 0; i < constraintCount; ++i)
            {
                var sourceIndex = Unsafe.Add(ref firstSourceIndex, i);
                var targetIndex = constraintStart + i;
                // 请注意,我们不必费心检查源和目标是否相同。
                // 与其有用的频率相比,分支机构的成本足够大,以至于它只在实际的静电情况下提供帮助。
                // 而且,它的最大效益也是相当小的。
                BundleIndexing.GetBundleIndices(sourceIndex, out var sourceBundle, out var sourceInner);
                BundleIndexing.GetBundleIndices(targetIndex, out var targetBundle, out var targetInner);

                Move(
                    ref typedBodyReferencesCache[sourceBundle], ref typedPrestepCache[sourceBundle], ref typedAccumulatedImpulsesCache[sourceBundle],
                    indexToHandleCache[sourceIndex], sourceInner,
                    ref typedBodyReferencesTarget[targetBundle], ref typedPrestepTarget[targetBundle], ref typedAccumulatedImpulsesTarget[targetBundle],
                    ref typeBatch.IndexToHandle[targetIndex], targetInner, targetIndex, ref handlesToConstraints);

            }
        }

        internal unsafe sealed override void GatherActiveConstraints(Bodies bodies, Solver solver, ref QuickList<ConstraintHandle> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch)
        {
            ref var activeConstraintSet = ref solver.ActiveSet;
            ref var activeBodySet = ref bodies.ActiveSet;
            for (int i = startIndex; i < endIndex; ++i)
            {
                var sourceHandle = sourceHandles[i];
                targetTypeBatch.IndexToHandle[i] = sourceHandle;
                ref var location = ref solver.HandleToConstraint[sourceHandle.Value];
                Debug.Assert(targetTypeBatch.TypeId == location.TypeId, "Can only gather from batches of the same type.");
                Debug.Assert(location.SetIndex == 0, "Can only gather from the active set.");

                ref var sourceBatch = ref activeConstraintSet.Batches[location.BatchIndex];
                ref var sourceTypeBatch = ref sourceBatch.TypeBatches[sourceBatch.TypeIndexToTypeBatchIndex[location.TypeId]];
                BundleIndexing.GetBundleIndices(location.IndexInTypeBatch, out var sourceBundle, out var sourceInner);
                BundleIndexing.GetBundleIndices(i, out var targetBundle, out var targetInner);

                // 请注意,我们不直接复制主体参照或投影信息。投影信息是短暂的,对于休眠约束是不必要的。
                // 身体引用被转换成句柄,这样唤醒就可以很容易地追踪到身体的读取位置。
                GatherScatter.CopyLane(
                    ref Buffer<TPrestepData>.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner,
                    ref Buffer<TPrestepData>.Get(ref targetTypeBatch.PrestepData, targetBundle), targetInner);
                GatherScatter.CopyLane(
                    ref Buffer<TAccumulatedImpulse>.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner,
                    ref Buffer<TAccumulatedImpulse>.Get(ref targetTypeBatch.AccumulatedImpulses, targetBundle), targetInner);
                ref var sourceReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As<TBodyReferences, int>(ref Buffer<TBodyReferences>.Get(ref sourceTypeBatch.BodyReferences, sourceBundle)), sourceInner);
                ref var targetReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As<TBodyReferences, int>(ref Buffer<TBodyReferences>.Get(ref targetTypeBatch.BodyReferences, targetBundle)), targetInner);
                var offset = 0;
                for (int j = 0; j < bodiesPerConstraint; ++j)
                {
                    Unsafe.Add(ref targetReferencesLaneStart, offset) = activeBodySet.IndexToHandle[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Value;
                    offset += Vector<int>.Count;
                }
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void CopyIncompleteBundle(int sourceStart, int targetStart, int count, ref TypeBatch sourceTypeBatch, ref TypeBatch targetTypeBatch)
        {
            for (int i = 0; i < count; ++i)
            {
                // 请注意,此实现允许两个线程访问单个包。如果真的发生了,这将是一个非常糟糕的虚假分享的案例,但是
                // 这不会造成正确性问题。
                var sourceIndex = sourceStart + i;
                var targetIndex = targetStart + i;
                BundleIndexing.GetBundleIndices(sourceIndex, out var sourceBundle, out var sourceInner);
                BundleIndexing.GetBundleIndices(targetIndex, out var targetBundle, out var targetInner);
                GatherScatter.CopyLane(
                    ref Buffer<TPrestepData>.Get(ref sourceTypeBatch.PrestepData, sourceBundle), sourceInner,
                    ref Buffer<TPrestepData>.Get(ref targetTypeBatch.PrestepData, targetBundle), targetInner);
                GatherScatter.CopyLane(
                    ref Buffer<TAccumulatedImpulse>.Get(ref sourceTypeBatch.AccumulatedImpulses, sourceBundle), sourceInner,
                    ref Buffer<TAccumulatedImpulse>.Get(ref targetTypeBatch.AccumulatedImpulses, targetBundle), targetInner);
            }
        }


        internal unsafe sealed override void CopySleepingToActive(
            int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetBatchIndex, int targetTypeBatchIndex,
            int sourceStart, int targetStart, int count, Bodies bodies, Solver solver)
        {
            ref var sourceTypeBatch = ref solver.Sets[sourceSet].Batches[sourceBatchIndex].TypeBatches[sourceTypeBatchIndex];
            ref var targetTypeBatch = ref solver.ActiveSet.Batches[targetBatchIndex].TypeBatches[targetTypeBatchIndex];
            Debug.Assert(sourceStart >= 0 && sourceStart + count <= sourceTypeBatch.ConstraintCount);
            Debug.Assert(targetStart >= 0 && targetStart + count <= targetTypeBatch.ConstraintCount,
                "This function should only be used when a region has been preallocated within the type batch.");
            Debug.Assert(sourceTypeBatch.TypeId == targetTypeBatch.TypeId);
            // TODO：请注意,在这里我们很容易放弃批量复制。
            // 如果您在性能分析中看到这一点显示得很有意义,请考虑执行不完整复制,以允许进行集中批量复制。
            // 这样的事情没有实现的唯一原因是简单性和时间。
            if ((targetStart & BundleIndexing.VectorMask) == 0 &&
                (sourceStart & BundleIndexing.VectorMask) == 0 &&
                ((count & BundleIndexing.VectorMask) == 0 || count == targetTypeBatch.ConstraintCount))
            {
                // 我们可以在这里使用简单的批量复制。
                Debug.Assert(count > 0);
                var bundleCount = BundleIndexing.GetBundleCount(count);
                var sourcePrestepData = sourceTypeBatch.PrestepData.As<TPrestepData>();
                var sourceAccumulatedImpulses = sourceTypeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
                var targetPrestepData = targetTypeBatch.PrestepData.As<TPrestepData>();
                var targetAccumulatedImpulses = targetTypeBatch.AccumulatedImpulses.As<TAccumulatedImpulse>();
                var sourceBundleStart = sourceStart >> BundleIndexing.VectorShift;
                var targetBundleStart = targetStart >> BundleIndexing.VectorShift;
                sourcePrestepData.CopyTo(sourceBundleStart, targetPrestepData, targetBundleStart, bundleCount);
                sourceAccumulatedImpulses.CopyTo(sourceBundleStart, targetAccumulatedImpulses, targetBundleStart, bundleCount);
            }
            else
            {
                CopyIncompleteBundle(sourceStart, targetStart, count, ref sourceTypeBatch, ref targetTypeBatch);
            }
            // 请注意,不能批量复制主体参照,因为非活动约束是指主体控制柄,而活动约束是指主体索引。
            for (int i = 0; i < count; ++i)
            {
                var sourceIndex = sourceStart + i;
                var targetIndex = targetStart + i;
                BundleIndexing.GetBundleIndices(sourceIndex, out var sourceBundle, out var sourceInner);
                BundleIndexing.GetBundleIndices(targetIndex, out var targetBundle, out var targetInner);

                ref var sourceReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As<TBodyReferences, int>(ref Buffer<TBodyReferences>.Get(ref sourceTypeBatch.BodyReferences, sourceBundle)), sourceInner);
                ref var targetReferencesLaneStart = ref Unsafe.Add(ref Unsafe.As<TBodyReferences, int>(ref Buffer<TBodyReferences>.Get(ref targetTypeBatch.BodyReferences, targetBundle)), targetInner);
                var offset = 0;
                for (int j = 0; j < bodiesPerConstraint; ++j)
                {
                    Unsafe.Add(ref targetReferencesLaneStart, offset) = bodies.HandleToLocation[Unsafe.Add(ref sourceReferencesLaneStart, offset)].Index;
                    offset += Vector<int>.Count;
                }
                var constraintHandle = sourceTypeBatch.IndexToHandle[sourceIndex];
                ref var location = ref solver.HandleToConstraint[constraintHandle.Value];
                Debug.Assert(location.SetIndex == sourceSet);
                location.SetIndex = 0;
                location.BatchIndex = targetBatchIndex;
                Debug.Assert(sourceTypeBatch.TypeId == location.TypeId);
                location.IndexInTypeBatch = targetIndex;
                // 这可以通过批量复制来完成,但是,嗯！我们已经触及了记忆。
                targetTypeBatch.IndexToHandle[targetIndex] = constraintHandle;
            }
        }


        internal unsafe sealed override void AddWakingBodyHandlesToBatchReferences(ref TypeBatch typeBatch, ref IndexSet targetBatchReferencedHandles)
        {
            for (int i = 0; i < typeBatch.ConstraintCount; ++i)
            {
                BundleIndexing.GetBundleIndices(i, out var sourceBundle, out var sourceInner);
                ref var sourceHandlesStart = ref Unsafe.Add(ref Unsafe.As<TBodyReferences, int>(ref Buffer<TBodyReferences>.Get(ref typeBatch.BodyReferences, sourceBundle)), sourceInner);
                var offset = 0;
                for (int j = 0; j < bodiesPerConstraint; ++j)
                {
                    var bodyHandle = Unsafe.Add(ref sourceHandlesStart, offset);
                    Debug.Assert(!targetBatchReferencedHandles.Contains(bodyHandle),
                        "It should be impossible for a batch in the active set to already contain a reference to a body that is being woken up.");
                    // 假设我们只添加了对已经存在的主体的引用,因此在活动集中的某个点上,它应该永远不会是必要的
                    // 若要调整批次引用句柄结构的大小,请执行以下操作。
                    targetBatchReferencedHandles.AddUnsafely(bodyHandle);
                    offset += Vector<int>.Count;
                }
            }
        }

        internal override int GetBodyReferenceCount(ref TypeBatch typeBatch, int bodyToFind)
        {
            // 这是一个纯粹的调试函数;性能无关紧要。
            // 请注意,此函数可用于活动集和非活动集。在激活集中,主体引用引用Bodies.ActiveSet中的*INDEX*。
            // 对于非活动约束集,主体参照为Body*Handles*。希望此功能的用户能够理解其中的差异。
            var bundleCount = typeBatch.BundleCount;
            var bodyReferences = typeBatch.BodyReferences.As<TBodyReferences>();
            int count = 0;
            var bodiesPerConstraint = InternalBodiesPerConstraint;
            for (int bundleIndex = 0; bundleIndex < bundleCount; ++bundleIndex)
            {
                var bundleSize = Math.Min(Vector<float>.Count, typeBatch.ConstraintCount - (bundleIndex << BundleIndexing.VectorShift));
                ref var bundleBase = ref Unsafe.As<TBodyReferences, Vector<int>>(ref bodyReferences[bundleIndex]);
                for (int constraintBodyIndex = 0; constraintBodyIndex < bodiesPerConstraint; ++constraintBodyIndex)
                {
                    ref var bodyVectorBase = ref Unsafe.As<Vector<int>, int>(ref Unsafe.Add(ref bundleBase, constraintBodyIndex));
                    for (int innerIndex = 0; innerIndex < bundleSize; ++innerIndex)
                    {
                        if (Unsafe.Add(ref bodyVectorBase, innerIndex) == bodyToFind)
                            ++count;
                        Debug.Assert(count <= 1);
                    }
                }
            }
            return count;
        }
    }


}
